

// This program uses functions to move and turn.  Each function accepts some parameters.
// Functions are usefull for saving space when you need to perform the same actions over and over.

//----------------------------------------------| MOVE |----------------------------------------------
#include "JoystickDriver.c"
#include "IRseeker/drivers/HTGYRO-driver.h"
int SPEED = (300) ;
float TIME10FEET = (3.9);
float TIME5ROTATIONS = (9.5);
int DEGREEOFFSET = (15);

// int SPEED = (50) ;
// float TIME10FEET = (4.3);
// float TIME5ROTATIONS = (10.0);
// int DEGREEOFFSET = (25);

long curAngle20 = 0;
int gyro = 0;
bool resetGyro = false;
int gyroCal = 0;
float curAngle = 0.0 ;
float driftrate = 0;
task gyroReader()
{
  while (true)
  {
    { HTGYROstartCal(HTGYRO);
      resetGyro = false;
      curAngle20 = 0;
      float drift = 0.;
      bool caldrift = true;
      int caldriftcount = 0;
      driftrate = 0;
      while (false == resetGyro)
      {
        gyro = HTGYROreadRot (HTGYRO);
        gyroCal = HTGYROreadCal(HTGYRO);
        curAngle20 = curAngle20 + gyro;
        drift = drift + driftrate;
        curAngle = (curAngle20 - drift)/ 20.;
        wait1Msec (50);
        if (true == caldrift)
        {caldriftcount = caldriftcount + 1;
          if (caldriftcount == 20)
          {driftrate = curAngle;
            curAngle = 0;
            caldrift = false;
          }
        }
      }
    }
  }
}


int BLUE = 2;
int RED = 3;
int color = BLUE;

void InitAuton()
{
  StartTask (gyroReader);
  waitForStart();
}
void Face (float goal)
{ float DEADZONE = 1;
  float speed = 30;
  bool stoploop = false;
  while (false == stoploop )
  {float error = (goal - curAngle);
    // tells the gyro how far we want it to go
    while (error > 180)
    { error = error - 360;
    }
    while (error < -180)
    { error = error + 360;
    }
    speed = abs(error) * 1.5;
    if (speed < 10)
    {speed = 10;
    }
    // ^ keeps values between -180 and 180
    if (error > DEADZONE)
    { //turn right
      motor[motorLEFT] = -speed;
      motor[motorRIGHT]  = -speed;
    }
    else if (error < -DEADZONE)
    { //turn left
      motor[motorLEFT] = speed;
      motor[motorRIGHT]  = speed;
    }
    else//if (error == DEADZONE)
    { stoploop = true;
    }
    //^ tells gyro what to do at every value
    wait10Msec (5);
  }
}
void goForward (float feet)
{
  float feetPerMSecond = 10 / TIME10FEET / 1000 ;
  int timeMS = feet / feetPerMSecond ;
  motor[motorLEFT] = - SPEED ;	// set mtr_S1_C1_1 to speed, 'rightSpeed'
  motor[motorRIGHT]  = SPEED;   	// set mtr_S1_C1_2 to speed, 'leftSpeed'
  writeDebugStream ("Fwd %3d ft %4d ms\n", feet, timeMS);
  wait1Msec(timeMS);
}
void gateup ()
{servo [gateleft] = 81;
  servo [gateright] = 145;
}
void gatedown ()
{servo [gateleft] = 230;
  servo [gateright] = 0;
}

void stopmotor ()
{
  motor[motorLEFT] =(0);	// set mtr_S1_C1_1 to speed, 'rightSpeed'
  motor[motorRIGHT]  =(0);// set mtr_S1_C1_2 to speed, 'leftSpeed'
  writeDebugStream ("Stop/n");
  wait1Msec (1000);
}

//----------------------------------------------------------------------------------------------------

//----------------------------------------------| TURN |----------------------------------------------
void turnright(int degrees)
{
  degrees=degrees+DEGREEOFFSET;
  float degreesPerMSecond = 1800 / TIME5ROTATIONS / 1000 ;
  int timeMS= degrees / degreesPerMSecond ;
  if (color == BLUE)
{
  motor[motorLEFT] = -SPEED;	// set mtr_S1_C1_1 to speed, 'rightSpeed'
  motor[motorRIGHT]  = -SPEED;// set mtr_S1_C1_2 to speed, 'leftSpeed'
}
else
{
  motor[motorLEFT] = SPEED;	// set mtr_S1_C1_1 to speed, 'rightSpeed'
  motor[motorRIGHT]  = SPEED;// set mtr_S1_C1_2 to speed, 'leftSpeed'
}
  writeDebugStream ("T %3d d %4d ms\n", degrees, timeMS);
  wait1Msec(timeMS);
}
//----------------------------------------------------------------------------------------------------
void turnleft(int degrees)
{
  degrees=degrees+DEGREEOFFSET;
  float degreesPerMSecond = 1800 / TIME5ROTATIONS / 1000 ;
  int timeMS= degrees / degreesPerMSecond ;
  if (color == BLUE)
{
  motor[motorLEFT] = SPEED;	// set mtr_S1_C1_1 to speed, 'rightSpeed'
  motor[motorRIGHT]  = SPEED;// set mtr_S1_C1_2 to speed, 'leftSpeed'
}
else
{
  motor[motorLEFT] = -SPEED;	// set mtr_S1_C1_1 to speed, 'rightSpeed'
  motor[motorRIGHT]  = -SPEED;// set mtr_S1_C1_2 to speed, 'leftSpeed'
}
  writeDebugStream ("T %3d d %4d ms\n", degrees, timeMS);
  wait1Msec(timeMS);
}
//---------------------------------------------------------------------------------------------------
void goBackward (float feet)
{
  float feetPerMSecond = 10 / TIME10FEET / 1000 ;
  int timeMS = feet / feetPerMSecond ;
  motor[motorLEFT] =  SPEED ;	// set mtr_S1_C1_1 to speed, 'rightSpeed'
  motor[motorRIGHT]  = - SPEED;   	// set mtr_S1_C1_2 to speed, 'leftSpeed'
  writeDebugStream ("Fwd %3d ft %4d ms\n", feet, timeMS);
  wait1Msec(timeMS);
}

void AutoBbBack ()
{ InitAuton ();
  gateup();
  goForward(3);
  gatedown();
  goForward (1.5);
  Face(120);
  gateup ();
  goForward (4.5);
  gatedown ();
  stopmotor ();
  //should have the bowling ball in center cavity
  goBackward (4);
  Face (180);
  goBackward (4.5);
  goForward (1);
  //opponent's crates pushed 3ft from the wall
  Face (-45);
  gateup ();
  goForward (2);
  // pushed bowling ball into corner
  stopmotor ();
  gatedown ();
  goBackward (18);
  /*goForward (1.5);
  Face (-135);
  goForward(3);
  Face (135);
  goForward (4);
  Face (180);
  goForward (1.5);
  Face (90);
  goForward (6);*/
  // in front of blue crates with back facing to push them
  /*goBackward (3.5);
  turnright(45); //changed from turnright to turnleft
  goBackward (4);

  // crates with racket balls are in the protected area
  //'if true' will drive directly to the front corner without getting the other stack into the protected area
  //'if false' will get the other stack into the protected are and then drive into the front corner
  if (true)
  {
  turnright (90);
  //robot should be able to drive straight into front corner
  goForward (6);
  }
  else
  {
  goForward (8);
  turnleft (45);
  goBackward (2);
  turnright (45);
  goBackward (9);
  turnleft (90);
  goForward (5);
  }*/

}

void AutoFr ()
{StartTask (gyroReader);
  waitForStart();
  gateup();
  goForward(3); // call our move function with a speed of 25 (forward)
  gatedown();
  goForward (1.5);
  stopmotor ();
  turnright(90);
  gateup ();
  goForward (4.5);
  gatedown ();
  goForward (4.5);
  turnleft (180);

}
